#!/usr/bin/env python3.6
# coding=utf-8

import rospy;
from std_msgs.msg import String;
from sensor_msgs.msg import Image as Image_msg
from robotserver.msg import State;
from robotserver.msg import Target;
import serial;
import threading;

class TF():
    def __init__(self):
        self.ser = serial.Serial("/dev/ttyACM0", 9600);

        #建立ros节点
        rospy.init_node("trans", anonymous=True);
        self.pub_to_ui = rospy.Publisher("sen", String, queue_size=1);
        self.pub_to_app = rospy.Publisher("/robot/state", State, queue_size=1)

        self.appState = State();

        self.init();

    #线程执行函数
    def thread_spin(self):
        rospy.spin();

    #ui界面回调函数
    def ui_callback(self, msg):
        print(msg.data);
        database = msg.data;
    
        self.ser.write(database.encode());
    
    #app回调函数
    def app_callback(self, msg):
        appTarget = msg;
        print(appTarget)
        if appTarget.isAuto:
            pass;
        else:
            if appTarget.isLaunch:
                pass;
    
    #相机回调函数
    def cam_callback(self, msg):
        pass;

    #主函数
    def init(self):  
        #订阅消息
        rospy.Subscriber("chatter", String, self.ui_callback);
        rospy.Subscriber("/robot/target", Target, self.app_callback);
        rospy.Subscriber("cam", Image_msg, self.cam_callback);
    
        #开启线程
        t = threading.Thread(target=self.thread_spin);
        t.start();

        while not rospy.is_shutdown():
            #接收下位机报文
            data = self.ser.read(5);
            if data:
                re = data.decode('utf-8','ignore');
                #print(re);

                #发布消息至ui
                self.pub_to_ui.publish(re);
                #rospy.loginfo("publish to ui successfully");

                #发布消息至app
                self.appState.isReady = True;
                self.appState.yaw = 10;
                self.appState.pitch = 10;
                self.pub_to_app.publish(self.appState);
                #rospy.loginfo("pulish to app successfully");

if __name__ == '__main__':
    try:
        tf = TF();
    except rospy.ROSInterruptException:
        pass;